/********************************************************************
*   Copyright Hainan Cui
*   BSfM - Batched Incremental Structure-from-Motion.
*   Copyright (C) 2017  Hainan Cui <hncui at nlpr.ia.ac.cn>
*   version1 Date: 2017-08-01
    a. with tracks selection based on ranked tracks
*   version2 Date: 2017-09-07
    a. maximal error in triangulation;
    b. tracks selection consider repro-error;
    c. belive in RA sometimes
*   version3 Data: 2017-09-28
    a. add refineEG module
    b. adjust the options parameters for refineEG or reconstruction
*   version3 Data: 2018-01-12
    a. change the voting-based camera registration
    b. P3P CVPR 2011 ==> P3P CVPR2017
*********************************************************************/

#include "sfm/SfMer.h"
#include <glog/logging.h>
using namespace RVG;

int SfMSolver(REC3D::ImageInfos& imageInfos, REC3D::ImagePairMatches& imagePairMatches, REC3D::SparsePoints& sparsePoints, string SfMDir,
	    REC3D::SfMParams& sfmParams)
{
    RVG::SfMer BSfM;	

    /// params setting
    {
		BSfM.imageInfos = &imageInfos;
		BSfM.imagePairMatches = &imagePairMatches;
		BSfM.sparsePoints = &sparsePoints;

		BSfM.SfMDir = SfMDir;
		BSfM.options_.Kcover_ = sfmParams.k_cover;
		BSfM.options_.addRatio_points = sfmParams.rigister_ratio;
		BSfM.options_.RAconf_ = sfmParams.ra_conf;
		BSfM.options_.minimum_P3P_matches = sfmParams.min_P3P_matches;
        BSfM.options_.minimum_P3P_inliers = sfmParams.min_P3P_inliers;
		BSfM.options_.minimum_tri_angle = sfmParams.min_tri_angle;
		BSfM.options_.minimum_visible_views = sfmParams.min_vis_views;
		BSfM.options_.nMST = sfmParams.n_OMST;

        boost::filesystem::create_directory(BSfM.SfMDir.c_str());
        boost::filesystem::create_directory(BSfM.SfMDir + "/Seed");
        boost::filesystem::create_directory(BSfM.SfMDir + "/Reg");
        boost::filesystem::create_directory(BSfM.SfMDir + "/Final");

        if(BSfM.options_.extract_color_)
            LOG(INFO) << "extract colors for the final scene" << std::endl;
        else
            LOG(INFO) << "no colors for the final scene" << std::endl;

        if(!BSfM.options_.tracks_selection_)
            LOG(INFO) << "no tracks selection" << std::endl;
        else
            LOG(INFO) << "tracks selection is working" << std::endl;

        if(BSfM.PreExtrinsic != "")
            BSfM.options_.preCalibration = true;
    }

    RVG::Timer SfMTimer;
    SfMTimer.Start();
    {
        int num_GNSS = 0;
        for(auto& imgInfo : *BSfM.imageInfos){
            if(imgInfo.cx != 0.0 && imgInfo.cy != 0.0 && imgInfo.cz != 0.0 && imgInfo.R.size() == 9){
                num_GNSS++;
                //LOG(INFO) << imgInfo.R[0] << " " << imgInfo.R[3] << " " << imgInfo.R[6] << std::endl;
            }
        }
        LOG(INFO) << "# of GNSS: " << num_GNSS << std::endl;
        if(num_GNSS >= 3)
            BSfM.options_.GNSS_Flag = true;
    }

    SfMTimer.Restart();
    {
        /// Loading datasets
        RVG::PrintHeading("Loading Cameras & Images");
        if(BSfM.LoadSQLDataBase())
        {
			RVG::PrintHeading("Parameters Setting");
			LOG(INFO) << "minimum_P3P_matches: " << BSfM.options_.minimum_P3P_matches << std::endl
				<< "minimum_tri_angle: " << BSfM.options_.minimum_tri_angle << std::endl
				<< "maximum_projection_error: " << BSfM.options_.maximum_projection_error << std::endl
				<< "minimum_visible_views: " << BSfM.options_.minimum_visible_views << std::endl
				<< "K_cover: " << BSfM.options_.Kcover_ << std::endl
				<< "regstration_points_ratio: " << BSfM.options_.addRatio_points << std::endl
				<< "thread: " << omp_get_max_threads() << std::endl;
            LOG(INFO) << "Loading Data Spent: " <<  SfMTimer.ElapsedSeconds() << " [seconds]"<< std::endl;
            BSfM.runningTimes["02-Loading Cameras & Images"] = SfMTimer.ElapsedSeconds();
        }
        else
        {
            return 0;
        }
    }

    SfMTimer.Restart();
    {
        if(BSfM.options_.preCalibration == false && BSfM.options_.GNSS_Flag == false)
        {
            RVG::PrintHeading("Seed Reconstruction");
            if(!BSfM.SeedReconstruction())
                return 0;
            BSfM.ImgMatches.clear();
            LOG(INFO) << "Seed Reconstruction Spent: " <<  SfMTimer.ElapsedSeconds() << " [seconds]"<< std::endl;
            BSfM.runningTimes["04-Seed Reconstruction"] = SfMTimer.ElapsedSeconds();
            BSfM.options_.num_of_prev_images = BSfM.calibImages.size();
            BSfM.options_.num_of_prev_points = BSfM.effTracks_.size();
        }
        else
        {
            if(BSfM.SetPrecalibration())
            {
                RVG::PrintHeading("Some Cameras are Pre-Calibrated");
                
                uint32_t initial_K = BSfM.options_.Kcover_;
                if(sfmParams.points_level == 0){
                    BSfM.options_.tracks_selection_ = false;
                }
                else{
                    BSfM.options_.Kcover_ = sfmParams.points_level;
                }

                if(BSfM.options_.gpsPrior == true)
                {
                    Eigen::Vector2d error = BSfM.TransformFrame2GPS(sfmParams.gps_err);
                    if(error(0) != 0.0)
                        BSfM.options_.transGPS = true;
                    else
                        BSfM.options_.transGPS = false;
                    BSfM.options_.RAconf_ = 0;
                    BSfM.options_.maximum_camera_registration = BSfM.images_.size();

                    LOG(INFO) << "Transform the current frame to GPS ECEF system" << std::endl;
                    BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                    //BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                    BSfM.BundleAdjustment_AngleAxis(false, false, false, 20);
                    
                    if(BSfM.calibImages.size() == BSfM.images_.size())
                    {
                        BSfM.global_iteration = -1;
                        BSfM.ExportResult(-2);
                        BSfM.TransformFrame2GPS(sfmParams.gps_err);
                        return 1;
                    }
                    else{
                        BSfM.options_.tracks_selection_ = true;
                        BSfM.options_.Kcover_ = initial_K;
                        BSfM.ExportResult(-1);
                    }
                }
                else
                {
                    BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                    BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                    BSfM.ExportResult(-2);
                }

                BSfM.options_.num_of_prev_images = BSfM.calibImages.size();
                BSfM.options_.num_of_prev_points = BSfM.effTracks_.size();               
            }
            else
                return 0;
        }
        BSfM.imageInfos->clear();
    }

    SfMTimer.Restart();
    {
        RVG::PrintHeading("Incremental Camera Registration");
        size_t preCalibrateCameras = BSfM.calibImages.size();
        size_t maxIteration = 2;
        int RAconf = BSfM.options_.RAconf_;
        bool finalT = false;
        int lastGrow = 0;

        while(BSfM.ImageAddition())
        {
            /// Local BA
            BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
            BSfM.LocalBundleAdjustment(false, false, false, BSfM.options_.baTimes_);

            finalT = false;
            maxIteration = 2;
            while(finalT == false && maxIteration--)
            {
                finalT = BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.LocalBundleAdjustment(false, false, false, BSfM.options_.baTimes_);
            }
            LOG(INFO) << ++BSfM.local_iteration << " Local BA is done!"<< std::endl;

            /// Global BA
            if(BSfM.calibImages.size() < 128)
            {
                lastGrow = BSfM.calibImages.size();
                BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                LOG(INFO) << ++BSfM.global_iteration << " global BA is done!"<< std::endl;
                BSfM.ExportResult(BSfM.global_iteration);
            }
            else if(BSfM.calibImages.size() >= lastGrow * 1.03)
            {
                lastGrow = BSfM.calibImages.size();
                BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                LOG(INFO) << ++BSfM.global_iteration << " global BA is done!"<< std::endl;
                BSfM.ExportResult(BSfM.global_iteration);
            }

            LOG(INFO) << "Last Iteration: " << preCalibrateCameras << ", Current Iteration: " << BSfM.calibImages.size() << std::endl;

            /// update
            if(RAconf != 0 && BSfM.options_.RAconf_ == 0)
            {
                RAconf = 0; // in the guodu iteration
                continue;
            }
            else
            {
                if(preCalibrateCameras > BSfM.calibImages.size())
                    break;
                else
                    preCalibrateCameras = BSfM.calibImages.size() > preCalibrateCameras ? BSfM.calibImages.size() : preCalibrateCameras;
            }
        }

        LOG(INFO) << "vital parameters are going to be loosed\n";
        BSfM.options_.minimum_tri_angle = 1.0;
        BSfM.options_.minimum_P3P_matches = 15;
        BSfM.options_.minimum_P3P_inliers = 12;
        BSfM.options_.P3P_inlier_ratio = 0.25;
        BSfM.options_.Kcover_ = 2.0 * BSfM.options_.Kcover_;
        BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
        BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);

        while(BSfM.ImageAddition())
        {
            /// Local BA
            BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
            BSfM.LocalBundleAdjustment(false, false, false, BSfM.options_.baTimes_);

            finalT = false;
            maxIteration = 2;
            while(finalT == false && maxIteration--)
            {
                finalT = BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.LocalBundleAdjustment(false, false, false, BSfM.options_.baTimes_);
            }
            LOG(INFO) << ++BSfM.local_iteration << " Local BA is done!"<< std::endl;


            /// Global BA
            if(BSfM.calibImages.size() >= lastGrow * 1.03)
            {
                lastGrow = BSfM.calibImages.size();
                BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                LOG(INFO) << ++BSfM.global_iteration << " global BA is done!"<< std::endl;
            }
            BSfM.ExportResult(BSfM.global_iteration);
            LOG(INFO) << "Last Iteration: " << preCalibrateCameras << ", Current Iteration: " << BSfM.calibImages.size() << std::endl;

            /// update
            if(preCalibrateCameras > BSfM.calibImages.size())
                break;
            else
                preCalibrateCameras = BSfM.calibImages.size() > preCalibrateCameras ? BSfM.calibImages.size() : preCalibrateCameras;
        }

        LOG(INFO) << "Camera Registration Spent: " <<  SfMTimer.ElapsedSeconds() << " [seconds]"<< std::endl;
        BSfM.runningTimes["05-Camera Calibration"] = SfMTimer.ElapsedSeconds();
        BSfM.ExportResult(-1);
    }

    SfMTimer.Restart();
    {
        BSfM.options_.minimum_tri_angle = 1.5;
        BSfM.options_.baTimes_ = 20;
        BSfM.options_.maximum_projection_error = 12.0;

        if(sfmParams.points_level == 0){
            BSfM.options_.tracks_selection_ = false;
        }
        else{
            BSfM.options_.Kcover_ = sfmParams.points_level;
        }
            
       
        RVG::PrintHeading("Final Bundle Adjustment (adjust all camera parameters)");
        bool finalT = false;
        int maxIteration = 2;
        while(finalT == false && maxIteration--)
        {
            finalT = BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
            BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
        }

        if(BSfM.options_.gpsPrior == true)
        {
            Eigen::Vector2d error = BSfM.TransformFrame2GPS(sfmParams.gps_err);
            if(error(0) == 0.0)
            {
                LOG(INFO) << "GPS transformation error is too large, the transformation failed, another BA is performed."<< std::endl;
                BSfM.TriangulateImages_RANSAC(BSfM.options_.tracks_selection_);
                BSfM.BundleAdjustment_AngleAxis(false, false, false, BSfM.options_.baTimes_);
                BSfM.TransformFrame2GPS(sfmParams.gps_err, true);
            }
        }

        LOG(INFO) << "Final BA Spent: " <<  SfMTimer.ElapsedSeconds() << " [seconds]"<< std::endl;
        BSfM.runningTimes["06-Final BA"] = SfMTimer.ElapsedSeconds();

        /// Output running-time and final SfM result
        BSfM.global_iteration = -1;
        BSfM.ExportRunningTimes();
        BSfM.ExportResult(-2);
        if (sfmParams.sfm_report)
        {
            BSfM.ExportReport();
        }
    }
    RVG::PrintHeading("SfM is Done");
    return 1;
}
